#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>

#include "test_interfaces/msg/task_feedback.hpp"
#include "test_interfaces/msg/task_result.hpp"
#include "test_interfaces/action/task_test.hpp"
#include "std_msgs/msg/string.hpp" // 包含标准消息中的字符串消息

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

class TaskActionClient : public rclcpp::Node
{
public:
    using TaskTest = test_interfaces::action::TaskTest;
    using GoalHandleTaskTest = rclcpp_action::ClientGoalHandle<TaskTest>;

    explicit TaskActionClient(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
        : Node("task_action_client", options)
    {
        this->client_ptr_ = rclcpp_action::create_client<TaskTest>(
            this,
            "TaskTest");

        this->timer_ = this->create_wall_timer(
            std::chrono::milliseconds(500),
            std::bind(&TaskActionClient::send_goal, this));
    }

    void send_goal()
    {
        using namespace std::placeholders;

        this->timer_->cancel();

        if (!this->client_ptr_->wait_for_action_server())
        {
            RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
            rclcpp::shutdown();
        }

        auto goal_msg = TaskTest::Goal();
        goal_msg.type = goal_msg.PICK;
        goal_msg.angle = 90;

        RCLCPP_INFO(this->get_logger(), "Sending goal");

        auto send_goal_options = rclcpp_action::Client<TaskTest>::SendGoalOptions();
        send_goal_options.goal_response_callback = std::bind(&TaskActionClient::goal_response_callback, this, _1);
        send_goal_options.feedback_callback = std::bind(&TaskActionClient::feedback_callback, this, _1, _2);
        send_goal_options.result_callback = std::bind(&TaskActionClient::result_callback, this, _1);

        this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
    }

private:
    rclcpp_action::Client<TaskTest>::SharedPtr client_ptr_;
    rclcpp::TimerBase::SharedPtr timer_;

    void goal_response_callback(const GoalHandleTaskTest::SharedPtr &goal_handle)
    {
        if (!goal_handle)
        {
            RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
        }
    }

    void feedback_callback(
        GoalHandleTaskTest::SharedPtr,
        const std::shared_ptr<const TaskTest::Feedback> feedback)
    {
        uint8_t status = feedback->feed.status;
        std::string msg = feedback->feed.msg;
        
        RCLCPP_INFO(this->get_logger(), "feedback status is %d, msg is %s", status, msg);
    }

    void result_callback(const GoalHandleTaskTest::WrappedResult &result)
    {
        switch (result.code)
        {
        case rclcpp_action::ResultCode::SUCCEEDED:
            break;
        case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
            return;
        case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
            return;
        default:
            RCLCPP_ERROR(this->get_logger(), "Unknown result code");
            return;
        }
        test_interfaces::msg::TaskResult ret = result.result->result;
        
        RCLCPP_INFO(this->get_logger(), "result code is %d, data is %s", ret.code, ret.data);
        rclcpp::shutdown();
    }
};